#include "Arduino.h"
#include "motor.h"

motor::motor(void) {

  _init = false;
  f_bias = 0;
  b_bias = 0;
}

void motor::begin(int sby, int in1, int in2, int pwm, int f_off, int b_off) {

  f_bias = f_off;
  b_bias = b_off;
  begin(sby, in1, in2, pwm);
}

void motor::begin(int sby, int in1, int in2, int pwm, int f_off) {

  f_bias = f_off;
  begin(sby, in1, in2, pwm);
}

void motor::begin(int sby, int in1, int in2, int pwm) {

  pinMode(sby, OUTPUT);
  digitalWrite(sby, LOW);
  SBYpin = sby;

  pinMode(in1, OUTPUT);
  digitalWrite(in1, LOW);
  IN1pin = in1;

  pinMode(in2, OUTPUT);
  digitalWrite(in2, LOW);
  IN2pin = in2;

  pinMode(pwm, OUTPUT);
  analogWrite(pwm, 0);
  PWMpin = pwm;

  _init = true;

  digitalWrite(sby, HIGH);
}

void motor::command(int cmd) {

  if (_init) {
    
    if (cmd > 0) {
      cmd += f_bias + b_bias;
      digitalWrite(IN2pin, LOW);
      digitalWrite(IN1pin, HIGH);
    } else if (cmd < 0) {
      cmd += b_bias;
      cmd = f_bias - cmd;
      digitalWrite(IN1pin, LOW);
      digitalWrite(IN2pin, HIGH);
    } else {
      cmd += b_bias;
      digitalWrite(IN2pin, LOW);
      digitalWrite(IN1pin, HIGH);
    }
   
    cmd = constrain(cmd, 0, 255);
    analogWrite(PWMpin, cmd);
  }

}